#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "ethercat.h"
#include <stdbool.h>

#ifdef _WIN32
#include <windows.h>  // Use Sleep function for Windows system
#else
#include <unistd.h>   // Use usleep function for Linux system
#endif

#define SLAVE_POSITION 1  // Position of the slave


// Delay function
void delay_ms(int milliseconds) {
#ifdef _WIN32
    Sleep(milliseconds);  // Use Sleep for Windows, unit is milliseconds
#else
    usleep(milliseconds * 1000);  // Use usleep for Linux, unit is microseconds
#endif
}

int SV630N(char* ifname)
{
    // Initialize EtherCAT master
    if (ec_init(ifname)) {
        printf("EtherCAT initialization succeeded.\n");
    }
    else {
        printf("EtherCAT initialization failed.\n");
        return -1;
    }

    // Configure slaves
    if (ec_config_init(FALSE) > 0) {
        printf("EtherCAT configuration succeeded.\n");
    }
    else {
        printf("EtherCAT configuration failed.\n");
        ec_close();
        return -1;
    }

    // Wait for all slaves to enter OP state
    ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);

    bool val = true;  
    ec_SDOwrite(SLAVE_POSITION, 0x7013, 0x01, FALSE, sizeof(val), &val, EC_TIMEOUTRXM);  // Set mode to PP mode
    ec_SDOwrite(SLAVE_POSITION, 0x7013, 0x02, FALSE, sizeof(val), &val, EC_TIMEOUTRXM);  // Set mode to PP mode
    delay_ms(100);  // Delay 100 milliseconds
    printf("2 \n");    

    uint16_t controlWord = 0x000F;  // Prepare enable
    ec_SDOwrite(SLAVE_POSITION, 0x7010, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
    delay_ms(100);  // Delay 100 millisecondsAAAA
    printf("F \n");

    // bool val = true;  
    ec_SDOwrite(SLAVE_POSITION, 0x7013, 0x01, FALSE, sizeof(val), &val, EC_TIMEOUTRXM);  // Set mode to PP mode
    ec_SDOwrite(SLAVE_POSITION, 0x7013, 0x02, FALSE, sizeof(val), &val, EC_TIMEOUTRXM);  // Set mode to PP mode
    delay_ms(100);  // Delay 100 milliseconds
    printf("2 \n");

   

    // Close EtherCAT master
    ec_close();
    return 0;
}

char ifbuf[1024];
boolean printSDO = FALSE;
boolean printMAP = FALSE;

int main(int argc, char* argv[])
{
    ec_adaptert* adapter = NULL;
    printf("SOEM (Simple Open EtherCAT Master)\nSlaveinfo\n");

    if (argc > 1)
    {
        if ((argc > 2) && (strncmp(argv[2], "-sdo", sizeof("-sdo")) == 0)) printSDO = TRUE;
        if ((argc > 2) && (strncmp(argv[2], "-map", sizeof("-map")) == 0)) printMAP = TRUE;
        /* start slaveinfo */
        strcpy(ifbuf, argv[1]);
        SV630N(ifbuf);
    }
    else
    {
        printf("Usage: slaveinfo ifname [options]\nifname = eth0 for example\nOptions :\n -sdo : print SDO info\n -map : print mapping\n");
        /* Print the list */
        printf("Available adapters\n");
        adapter = ec_find_adapters();
        while (adapter != NULL)
        {
            printf("Description : %s, Device to use for wpcap: %s\n", adapter->desc, adapter->name);
            adapter = adapter->next;
        }
    }

    printf("End program\n");
    return (0);
}